#ifndef INS_PATHSEARCH_H
#define INS_PATHSEARCH_H

#endif //INS_PATHSEARCH_H
#define COST 10.0
#include <utility>
#include <unordered_set>
#include <set>
#include "map.h"
#include <find_path/findpath.h>
class Node{
public:
    int x,y;
    unsigned int order;
    Node * pre; //父节点
    float H; //节点到终点的估算
    float G; //起点到节点的代价
    float F; //代价
    int8_t priority; //是障碍物的概率
    Node(int x, int y) : x(x), y(y) {
        pre = nullptr;
    }
    ~Node(){
        delete(pre);
    }
};

//用堆实现取最小代价的点，set快速查找是否存在
class Openlist {
    vector<Node *> openList;
    std::unordered_set<int> openSet;
    static bool comp(Node* l1, Node* l2)
    {
        return l1->F > l2->F;   // min heap
    }
public:
    Node* getLessNode(){
        std::pop_heap(std::begin(openList),std::end(openList), comp);
        Node * result = openList.back();
        openList.pop_back();
        openSet.erase(result->order);
        return result;
    }
    Node * getNodeByorder(unsigned int order){
        for(Node* node :openList){
            if (node->order == order){
                return node;
            }
        }
        return nullptr;
    }
    void changeNode(unsigned int order,float G,Node * pre){
        for(Node* node :openList){
            if(node->order == order){
                if(G < node ->G){
                    node ->pre = pre;
                    node ->G = G;
                    node ->F = node->H + node ->G;
                    std::make_heap(std::begin(openList),std::end(openList), comp);
                }
            }
        }
    }
    void addNode(Node * n){
        openList.push_back(n);
        std::push_heap(std::begin(openList),std::end(openList), comp);
        openSet.insert(n->order);
    }
    bool isInOpenList(unsigned int order){
        return openSet.find(order)!=openSet.end();
    }
    bool isEmpty(){
        return openList.empty();
    }
    void clear(){
        openList.clear();
        openSet.clear();
    }
};
class aStarMethod{
    Map* const map;
    Openlist openlist;
    std::unordered_set<unsigned int> closeList;
    bool isInCloseList(unsigned int order){
        return closeList.find(order)!=closeList.end();
    }
public:
    explicit aStarMethod(Map *const map) : map(map) {
    }

    Node * creatNode(int x ,int y,Node *pre,Node *end, float typeCost){
        Node* n =new Node(x,y);
        n->priority = map->pixel[x][y];
        n->pre = pre;
        n->G = pre->G + ((pre->priority+n->priority)/200.0)*COST*typeCost;
        n->H = COST * sqrt(pow(abs(end->x - x),2)+pow(abs(end->y - y),2))*0.1; //欧式距离的十分之一
        n->F = n->G + n->H;
        n->order = x*map->width + y;
        return n;
    }
    void addOpenList(Node* pre,Node * end){
        for(int x=-1;x<2;x++){
            for(int y=-1;y<2;y++){
                if(x==0 && y==0){
                    continue;
                }
                float typeCost = 1.4;
                if(x== 0||y==0){
                    typeCost = 1;
                }
                int row = pre->x + x;
                int col = pre->y + y;
                unsigned int order =row*map->width + col;
                //大于90视为不可达
                if(!map->isInMap(row,col) ||map->pixel[row][col] >90 ||isInCloseList(order)){
                    continue;
                }
                if(openlist.isInOpenList(order)){
                    float G = pre->G + (pre->priority/100.0)*COST*typeCost;
                    openlist.changeNode(order,G,pre);
                } else{
                    openlist.addNode(creatNode(row,col,pre,end,typeCost));
                }
            }
        }
    }
    std::deque<unsigned int> findPath(int x1, int y1, int x2 ,int y2){
        std::deque<unsigned int> res;
        if(!map->isInMap(x1,y1) || map->pixel[x1][y1] >90||!map->isInMap(x2,y2) || map->pixel[x2][y2] >90){
            ROS_INFO("no path");
            return res;
        }
        Node start(x1,y1);
        start.G = 0;
        start.order = x1*map->width + y1;
        start.priority = map->pixel[x1][y1];
        Node end(x2,y2);
        end.H = 0;
        end.order = x2*map->width + y2;
        end.priority = map->pixel[x2][y2];
        closeList.insert(start.order);
        addOpenList(&start,&end);
        Node * endNode = nullptr;
        while (Node * n = openlist.getLessNode()){
            if(n->order == end.order){
                endNode=n;
                break;
            }
            if(openlist.isInOpenList(end.order)){
                endNode = openlist.getNodeByorder(end.order);
            }
            closeList.insert(n->order);
            addOpenList(n,&end);
            if(openlist.isEmpty()){
                openlist.clear();
                closeList.clear();
                return res;
            }
        }
        while(endNode!= nullptr){
            res.push_front(endNode->order);
            endNode=endNode->pre;
        }
        openlist.clear();
        closeList.clear();
        return res;
    }

    bool getPath(find_path::findpath::Request& req, find_path::findpath::Response& resp){
        map->pushMapTopic();
        resp.path = map->pushPathTopic(this->findPath(req.x1,req.y1,req.x2,req.y2));
        return true;
    }
};